#include "../H/DiskStorage.h"

//------------------------------------------------------------------------------
// METHOD:  DiskStorage::DiskStorage()
// Type:	Constructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
DiskStorage::DiskStorage()
{
	printf("BEGIN:  DiskStorage()\n");
	SmartDashboard::init();
	pLoaderEntrySwitch    = new DigitalInput(DIGITAL_SIDECAR_MODULE,LOADER_ENTRY_SW_CH);
	pLoaderBottomSwitch   = new DigitalInput(DIGITAL_SIDECAR_MODULE,LOADER_BOTTOM_SW_CH);
	pLoaderLowMidSwitch   = new DigitalInput(DIGITAL_SIDECAR_MODULE,LOADER_LOW_MID_SW_CH);
	pLoaderHighMidSwitch  = new DigitalInput(DIGITAL_SIDECAR_MODULE,LOADER_HIGH_MID_SW_CH);
	pLoaderHighSwitch     = new DigitalInput(DIGITAL_SIDECAR_MODULE,LOADER_HIGH_SW_CH);
	pHelixMotor			  = new Victor (DIGITAL_SIDECAR_MODULE,HELIX_MOTOR_CH);
	printf("END:    DiskStorage()\n");
}
//------------------------------------------------------------------------------
// METHOD:  DiskStorage::~DiskStorage()
// Type:	Destructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
DiskStorage::~DiskStorage()
{
	// Does nothing
}

//------------------------------------------------------------------------------
// METHOD:  DiskStorage::DetermineDiskLocation()
// Type:	Method
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
int DiskStorage::DetermineDiskAction()
{
	printf("BEGIN:  DiskStorage::DetermineDiskLocation()\n-----\n");

	SmartDashboard::PutBoolean("Disk Entering", diskEntering);
	SmartDashboard::PutBoolean("Disk One Loaded",diskOneLoaded);
	SmartDashboard::PutBoolean("Disk Two Loaded",diskTwoLoaded);
	SmartDashboard::PutBoolean("Disk Three Loaded",diskThreeLoaded);
	SmartDashboard::PutBoolean("Disk Four Loaded",diskFourLoaded);
	SmartDashboard::PutNumber("The Highest Disk Place is: ", highestDiskPlace);

	highestDiskPlace = 0;
	diskOneLoaded    = false;
	diskTwoLoaded    = false;
	diskThreeLoaded  = false;
	diskFourLoaded   = false;
	diskEntering	 = false;
	diskAction		 = NoActionNeeded;

	diskEntering = pLoaderEntrySwitch->Get();

	if(pLoaderBottomSwitch->Get())
	{
		diskOneLoaded    = true;
		highestDiskPlace = 1;

		if(pLoaderLowMidSwitch->Get())
		{
			diskTwoLoaded    = true;
			highestDiskPlace = 2;

			if(pLoaderHighMidSwitch->Get())
			{
				diskThreeLoaded  = true;
				highestDiskPlace = 3;

				if(pLoaderHighSwitch->Get())
				{
					diskFourLoaded   = true;
					highestDiskPlace = 4;
				}
			}
		}
	}
	else if (highestDiskPlace >=1 && !diskOneLoaded && !movingForward && finishedReadying)
		diskAction = DisksToReverse;
	else
		highestDiskPlace = 0;

	if ((diskEntering || movingForward)&& highestDiskPlace != 4 )
		diskAction = DisksToLoad;

	if (!finishedReadying)
		diskAction = DisksToReady;


	printf("END:    DiskStorage::DetermineDiskLocation()\n");
	return diskAction;
}

//------------------------------------------------------------------------------
// METHOD:  DiskStorage::LoadDisks()
// Type:	Method
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void  DiskStorage::LoadDisks()
{
	printf("BEGIN:  DiskStorage::LoadDisks()\n-----\n");
	if (highestDiskPlace != lastHighestDiskPlace + 1)
	{
		pHelixMotor->Set(HELIX_SPEED);
		movingForward = true;
	}
	else
	{
		pHelixMotor->Set(ALL_STOP);
		movingForward = false;
	}

	lastHighestDiskPlace = highestDiskPlace;

	printf("END:    DiskStorage::LoadDisks()\n");
	return;
}

//------------------------------------------------------------------------------
// METHOD:  DiskStorage::LoadDisks()
// Type:	Method
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
bool  DiskStorage::ReadyDisksForShooting()
{
	printf("BEGIN:  DiskStorage::LoadDisks()\n-----\n");
	if (highestDiskPlace != 4)
	{
		pHelixMotor->Set(HELIX_SPEED);
		finishedReadying = false;
	}
	else
	{
		pHelixMotor->Set(ALL_STOP);
		finishedReadying = true;
	}

	lastHighestDiskPlace = highestDiskPlace;

	printf("END:    DiskStorage::LoadDisks()\n");
	return finishedReadying;
}

//------------------------------------------------------------------------------
// METHOD:  DiskStorage::ReverseDisks()
// Type:	Method
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void  DiskStorage::ReverseDisks()
{
	printf("BEGIN:  DiskStorage::ReverseDisks()\n-----\n");

	pHelixMotor->Set(REVERSE_SPEED);

	printf("END:    DiskStorage::ReverseDisks()\n");
	return;
}
